#include "esp_i2c_master.h"

#include <algorithm>
#include <cstring>
#include <cstdio>

#include "esp_log.h"
#include "esp_check.h"
#include "driver/gpio.h"
#include "esp_rom_sys.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"



#ifndef I2C_ADDR_BIT_LEN_10
#define I2C_ADDR_BIT_LEN_10 I2C_ADDR_BIT_LEN_7  // 如果不支持10位地址，使用7位
#endif

static const char* TAG = "ESP32_I2C";

// 辅助函数：转换时钟源
static i2c_clock_source_t convertClockSource(I2CClockSource clk_src) {
    switch (clk_src) {
        case I2CClockSource::DEFAULT:
            return I2C_CLK_SRC_DEFAULT;
#if SOC_I2C_SUPPORT_XTAL
        case I2CClockSource::XTAL:
            return I2C_CLK_SRC_XTAL;
#endif
#if SOC_I2C_SUPPORT_RTC
        case I2CClockSource::RC_FAST:
            return I2C_CLK_SRC_RC_FAST;
#endif
#if SOC_I2C_SUPPORT_APB
        case I2CClockSource::APB:
            return I2C_CLK_SRC_APB;
#endif
        default:
            return I2C_CLK_SRC_DEFAULT;
    }
}

// I2cMaster 实现
I2cMaster::~I2cMaster() {
    cleanup();
    ESP_LOGI(TAG, "I2C删除成功，端口: %d", port_);
}

I2cMaster::I2cMaster(I2cMaster&& other) noexcept
    : type_(other.type_)
    , port_(other.port_)
    , scl_io_(other.scl_io_)
    , sda_io_(other.sda_io_)
    , initialized_(other.initialized_)
    , hw_bus_handle_(other.hw_bus_handle_)
    , device_handles_(std::move(other.device_handles_)) {
    
    other.hw_bus_handle_ = nullptr;
    other.initialized_ = false;
    other.device_handles_.clear();
}

I2cMaster& I2cMaster::operator=(I2cMaster&& other) noexcept {
    if (this != &other) {
        cleanup();
        
        type_ = other.type_;
        port_ = other.port_;
        scl_io_ = other.scl_io_;
        sda_io_ = other.sda_io_;
        initialized_ = other.initialized_;
        hw_bus_handle_ = other.hw_bus_handle_;
        device_handles_ = std::move(other.device_handles_);
        
        other.hw_bus_handle_ = nullptr;
        other.initialized_ = false;
        other.device_handles_.clear();
    }
    return *this;
}

esp_err_t I2cMaster::init(const I2CConfig& config) {
    // 初始化过程需要锁保护，防止并发初始化
    std::lock_guard<std::mutex> lock(mutex_);
    
    if (initialized_) {
        ESP_LOGW(TAG, "I2C已初始化");
        return ESP_ERR_INVALID_STATE;
    }
    
    if (config.scl_io_num == GPIO_NUM_NC || config.sda_io_num == GPIO_NUM_NC) {
        ESP_LOGE(TAG, "SCL和SDA引脚不能为GPIO_NUM_NC");
        return ESP_ERR_INVALID_ARG;
    }
    
    if (config.i2c_port < 0) {
        ESP_LOGE(TAG, "无效的I2C端口号: %d", config.i2c_port);
        return ESP_ERR_INVALID_ARG;
    }
    
    // 检查端口合理
    if (config.type == I2CType::HARDWARE) {
        if (config.i2c_port >= SOC_I2C_NUM) {
            ESP_LOGE(TAG, "硬件I2C端口号超出范围: %d", config.i2c_port);
            return ESP_ERR_INVALID_ARG;
        }
    }
    
    type_ = config.type;
    port_ = config.i2c_port;
    scl_io_ = config.scl_io_num;
    sda_io_ = config.sda_io_num;
    
    esp_err_t ret;
    if (type_ == I2CType::HARDWARE) {
        ret = initializeHardwareI2C(config);
    } else {
        ret = initializeSoftwareI2C(config);
    }
    
    if (ret == ESP_OK) {
        initialized_ = true;
        ESP_LOGI(TAG, "I2C创建成功，端口: %d, 类型: %s", 
               port_, (type_ == I2CType::HARDWARE) ? "硬件" : "软件");
    } else {
        // 注意：这里调用cleanup()会尝试获取锁，但我们已经持有锁
        // 需要创建一个内部清理函数
        cleanupInternal();
        ESP_LOGE(TAG, "I2C初始化失败: %s", errorToString(ret));
    }
    
    return ret;
}

esp_err_t I2cMaster::initializeHardwareI2C(const I2CConfig& config) {
    i2c_master_bus_config_t i2c_mst_config = {};
    i2c_mst_config.clk_source = convertClockSource(config.clk_source);
    i2c_mst_config.i2c_port = config.i2c_port;
    i2c_mst_config.scl_io_num = config.scl_io_num;
    i2c_mst_config.sda_io_num = config.sda_io_num;
    i2c_mst_config.glitch_ignore_cnt = config.glitch_ignore_cnt;
    i2c_mst_config.intr_priority = config.intr_priority;
    i2c_mst_config.trans_queue_depth = config.trans_queue_depth;
    i2c_mst_config.flags.enable_internal_pullup = config.enable_internal_pullup;
    i2c_mst_config.flags.allow_pd = config.allow_pd;
    
    esp_err_t ret = i2c_new_master_bus(&i2c_mst_config, &hw_bus_handle_);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "硬件I2C主总线创建失败: %s", errorToString(ret));
    }
    
    return ret;
}

esp_err_t I2cMaster::initializeSoftwareI2C(const I2CConfig& config) {
    // 创建软件I2C总线结构
    sw_bus_handle_ = new SoftI2CBus();
    if (!sw_bus_handle_) {
        ESP_LOGE(TAG, "软件I2C内存分配失败");
        return ESP_ERR_NO_MEM;
    }
    
    sw_bus_handle_->scl_pin = config.scl_io_num;
    sw_bus_handle_->sda_pin = config.sda_io_num;
    
    // 根据频率计算延迟
    uint32_t freq_hz = config.scl_speed_hz;
    if (freq_hz <= 100000) {
        sw_bus_handle_->freq_delay_us = 5;  // 100kHz
    } else if (freq_hz <= 200000) {
        sw_bus_handle_->freq_delay_us = 3;  // 200kHz
    } else {
        sw_bus_handle_->freq_delay_us = 2;  // 300kHz+
    }
    
    // 配置GPIO
    esp_err_t ret = gpio_set_direction(config.scl_io_num, GPIO_MODE_INPUT_OUTPUT_OD);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "配置SCL引脚失败: %s", errorToString(ret));
        delete sw_bus_handle_;
        sw_bus_handle_ = nullptr;
        return ret;
    }
    
    ret = gpio_set_direction(config.sda_io_num, GPIO_MODE_INPUT_OUTPUT_OD);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "配置SDA引脚失败: %s", errorToString(ret));
        delete sw_bus_handle_;
        sw_bus_handle_ = nullptr;
        return ret;
    }
    
    if (config.enable_internal_pullup) {
        gpio_set_pull_mode(config.scl_io_num, GPIO_PULLUP_ONLY);
        gpio_set_pull_mode(config.sda_io_num, GPIO_PULLUP_ONLY);
    }
    
    // 设置初始状态（空闲状态）
    gpio_set_level(config.scl_io_num, 1);
    gpio_set_level(config.sda_io_num, 1);
    
    sw_bus_handle_->initialized = true;
    return ESP_OK;
}

void I2cMaster::cleanup() {
    if (initialized_) {
        // 设备句柄管理需要锁保护
        std::lock_guard<std::mutex> lock(mutex_);
        cleanupInternal();
    }
}

void I2cMaster::cleanupInternal() {
    // 内部清理函数，调用前必须已持有锁
    if (type_ == I2CType::HARDWARE) {
        for (auto& pair : device_handles_) {
            i2c_master_bus_rm_device(pair.second);
        }
        device_handles_.clear();
        
        if (hw_bus_handle_ != nullptr) {
            i2c_del_master_bus(hw_bus_handle_);
            hw_bus_handle_ = nullptr;
        }
    } 
    else if (type_ == I2CType::SOFTWARE && sw_bus_handle_ != nullptr) {
        delete sw_bus_handle_;
        sw_bus_handle_ = nullptr;
    }
    initialized_ = false;
}

// 设备管理功能实现
i2c_master_dev_handle_t I2cMaster::addDevice(uint16_t device_address, uint32_t scl_speed_hz) {
    if (!initialized_ || type_ != I2CType::HARDWARE) {
        ESP_LOGE(TAG, "I2C未初始化或不是硬件I2C");
        return nullptr;
    }
    
    if (!isValidDeviceAddress(device_address, I2CAddrLength::ADDR_7BIT)) {
        ESP_LOGE(TAG, "无效的设备地址: 0x%02X", device_address);
        return nullptr;
    }
    
    std::lock_guard<std::mutex> lock(mutex_);
    
    // 检查设备是否已存在
    auto it = device_handles_.find(device_address);
    if (it != device_handles_.end()) {
        ESP_LOGW(TAG, "设备 0x%02X 已存在", device_address);
        return it->second;
    }
    
    // 创建新设备
    return createDeviceInternal(device_address, scl_speed_hz);
}

esp_err_t I2cMaster::removeDevice(i2c_master_dev_handle_t dev_handle) {
    if (!initialized_ || type_ != I2CType::HARDWARE || dev_handle == nullptr) {
        ESP_LOGE(TAG, "移除设备失败: 参数无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    std::lock_guard<std::mutex> lock(mutex_);
    
    // 查找并移除设备
    for (auto it = device_handles_.begin(); it != device_handles_.end(); ++it) {
        if (it->second == dev_handle) {
            esp_err_t ret = i2c_master_bus_rm_device(dev_handle);
            if (ret == ESP_OK) {
                ESP_LOGI(TAG, "移除设备成功: 0x%02X", it->first);
                device_handles_.erase(it);
            } else {
                ESP_LOGE(TAG, "移除设备失败: 0x%02X, 错误: %s", it->first, errorToString(ret));
            }
            return ret;
        }
    }
    
    ESP_LOGW(TAG, "未找到要移除的设备句柄");
    return ESP_ERR_NOT_FOUND;
}

// 获取或创建设备句柄（内部使用，调用前必须已持有锁）
i2c_master_dev_handle_t I2cMaster::getOrCreateDeviceHandle(uint16_t device_address, uint32_t scl_speed_hz) {
    if (type_ != I2CType::HARDWARE) {
        return nullptr;
    }
    
    // 首先尝试从缓存中获取
    auto it = device_handles_.find(device_address);
    if (it != device_handles_.end()) {
        return it->second;
    }
    
    // 如果不存在，直接创建新的设备句柄（不再调用addDevice避免死锁）
    return createDeviceInternal(device_address, scl_speed_hz);
}

// 内部创建设备函数（调用前必须已持有锁）
i2c_master_dev_handle_t I2cMaster::createDeviceInternal(uint16_t device_address, uint32_t scl_speed_hz) {
    // 创建新设备
    i2c_device_config_t i2c_dev_config = {};
    i2c_dev_config.dev_addr_length = I2C_ADDR_BIT_LEN_7;
    i2c_dev_config.device_address = device_address;
    i2c_dev_config.scl_speed_hz = scl_speed_hz;
    
    i2c_master_dev_handle_t dev_handle;
    esp_err_t ret = i2c_master_bus_add_device(hw_bus_handle_, &i2c_dev_config, &dev_handle);
    
    if (ret == ESP_OK) {
        device_handles_[device_address] = dev_handle;
        ESP_LOGI(TAG, "添加设备成功: 0x%02X, 频率: %lu Hz", device_address, scl_speed_hz);
        return dev_handle;
    } else {
        ESP_LOGE(TAG, "添加设备失败: 0x%02X, 错误: %s", device_address, errorToString(ret));
        return nullptr;
    }
}

bool I2cMaster::probeDevice(uint16_t device_address, int timeout_ms) {
    if (!initialized_) {
        ESP_LOGE(TAG, "I2C未初始化");
        return false;
    }
    
    if (device_address >= 0x80) {
        return false;
    }
    
    esp_err_t ret;
    if (type_ == I2CType::HARDWARE) {
        // i2c_master_probe 已经是线程安全的，不需要额外锁
        ret = i2c_master_probe(hw_bus_handle_, device_address, timeout_ms);
    } else {
        // 软件I2C需要锁保护
        std::lock_guard<std::mutex> lock(mutex_);
        ret = softI2CStart();
        if (ret == ESP_OK) {
            ret = softI2CWriteByte((device_address << 1) | 0);  // 写操作
            softI2CStop();
        }
    }
    
    bool found = (ret == ESP_OK);
    return found;
}

esp_err_t I2cMaster::scanBus(std::vector<uint8_t>& found_devices, int timeout_ms) {
    if (!initialized_) {
        ESP_LOGE(TAG, "I2C未初始化");
        return ESP_ERR_INVALID_STATE;
    }
    
    found_devices.clear();
    
    // 美化界面：添加分隔线和标题
    ESP_LOGI(TAG, "╔══════════════════════════════════════════════════════════╗");
    ESP_LOGI(TAG, "║                    I2C 总线扫描工具                      ║");
    ESP_LOGI(TAG, "╚══════════════════════════════════════════════════════════╝");
    
    ESP_LOGI(TAG, "⏺ 总线信息: 端口 %d (%s), SCL: GPIO %d, SDA: GPIO %d", 
             port_, (type_ == I2CType::HARDWARE) ? "硬件I2C" : "软件I2C", scl_io_, sda_io_);
    
    // 检查总线状态
    if (!isBusIdle()) {
        ESP_LOGW(TAG, "⚠ 警告: I2C总线可能忙碌，尝试重置总线...");
        resetBus();
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    ESP_LOGI(TAG, "⏺ 开始扫描，请等待...");
    ESP_LOGI(TAG, "┌─────┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┬──┐");
    ESP_LOGI(TAG, "│     │00│01│02│03│04│05│06│07│08│09│0a│0b│0c│0d│0e│0f│");
    ESP_LOGI(TAG, "├─────┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┤");
    
    int scan_count = 0;
    int timeout_count = 0;
    
    for (int i = 0; i < 128; i += 16) {
        char line_buffer[128];
        int pos = snprintf(line_buffer, sizeof(line_buffer), "│ %02x: │", i);
        
        for (int j = 0; j < 16; j++) {
            uint8_t address = i + j;
            scan_count++;
            
            // 跳过保留地址
            if (address < 0x08 || address >= 0x78) {
                pos += snprintf(line_buffer + pos, sizeof(line_buffer) - pos, "  │");
                continue;
            }
            
            if (probeDevice(address, timeout_ms)) {
                found_devices.push_back(address);
                // 使用"√√"表示设备存在
                pos += snprintf(line_buffer + pos, sizeof(line_buffer) - pos, "██│");
            } else {
                pos += snprintf(line_buffer + pos, sizeof(line_buffer) - pos, "  │");
                timeout_count++;
            }
            
            // 每扫描几个地址就给其他任务一些时间
            if (scan_count % 8 == 0) {
                vTaskDelay(pdMS_TO_TICKS(1));
            }
        }
        
        ESP_LOGI(TAG, "%s", line_buffer);
          // 在最后一行后添加底部边框，否则添加分隔线
        if (i == 112) {
            ESP_LOGI(TAG, "└─────┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┘");
        } else {
            ESP_LOGI(TAG, "├─────┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┼──┤");
        }
    }
    
    // 美化结果输出
    ESP_LOGI(TAG, "╔══════════════════════════════════════════════════════════╗");
    ESP_LOGI(TAG, "║                     扫描结果汇总                         ║");
    ESP_LOGI(TAG, "╚══════════════════════════════════════════════════════════╝");
    
    ESP_LOGI(TAG, "• 扫描统计: 扫描地址 = %d, 响应地址 = %zu, 无响应 = %d", 
             scan_count, found_devices.size(), timeout_count);
    
    if (found_devices.empty()) {
        ESP_LOGW(TAG, "⚠ 未找到任何I2C设备！");
        ESP_LOGW(TAG, "⚠ 可能的原因:");
        ESP_LOGW(TAG, "  • 没有连接I2C设备");
        ESP_LOGW(TAG, "  • 上拉电阻缺失或值不正确");
        ESP_LOGW(TAG, "  • 设备未正确供电");
        ESP_LOGW(TAG, "  • SCL/SDA引脚连接错误");
        ESP_LOGW(TAG, "  • I2C频率过高");
        ESP_LOGW(TAG, "⚠ 建议:");
        ESP_LOGW(TAG, "  • 检查硬件连接");
        ESP_LOGW(TAG, "  • 添加4.7kΩ上拉电阻");
        ESP_LOGW(TAG, "  • 尝试降低I2C频率到50kHz");
        ESP_LOGW(TAG, "  • 使用万用表检查SCL/SDA电压");
    } else {
        ESP_LOGI(TAG, "✅ 扫描完成，找到 %zu 个设备:", found_devices.size());
        
        // 表格形式显示设备信息
        ESP_LOGI(TAG, "┌──────┬────────┬───────────┐");
        ESP_LOGI(TAG, "│ 序号 │ 地址(H)│ 地址(D)   │");
        ESP_LOGI(TAG, "├──────┼────────┼───────────┤");
        
        int idx = 1;
        for (uint8_t addr : found_devices) {
            ESP_LOGI(TAG, "│ %2d   │  0x%02X  │   %3d    │", idx++, addr, addr);
        }
        
        ESP_LOGI(TAG, "└──────┴────────┴───────────┘");
        ESP_LOGI(TAG, "✅ 常见I2C设备地址参考:");
        ESP_LOGI(TAG, "• 0x3C/0x3D - SSD1306/SH1106 OLED显示屏");
        ESP_LOGI(TAG, "• 0x68/0x69 - MPU6050/MPU9250 加速度计");
        ESP_LOGI(TAG, "• 0x76/0x77 - BME280/BMP280 气压/温湿度传感器");
        ESP_LOGI(TAG, "• 0x57     - ADXL345 加速度计");
        ESP_LOGI(TAG, "• 0x48-0x4B - ADS1115/ADS1015 模数转换器");
        ESP_LOGI(TAG, "• 0x50-0x57 - AT24CXX EEPROM存储器");
    }
    
    return ESP_OK;
}

esp_err_t I2cMaster::resetBus() {
    if (!initialized_) {
        ESP_LOGE(TAG, "I2C未初始化");
        return ESP_ERR_INVALID_STATE;
    }
    
    // GPIO操作需要锁保护
    std::lock_guard<std::mutex> lock(mutex_);
    
    // 重新配置GPIO
    esp_err_t ret = gpio_set_direction(scl_io_, GPIO_MODE_INPUT_OUTPUT_OD);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "重置SCL引脚失败: %s", errorToString(ret));
        return ret;
    }
    
    ret = gpio_set_direction(sda_io_, GPIO_MODE_INPUT_OUTPUT_OD);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "重置SDA引脚失败: %s", errorToString(ret));
        return ret;
    }
    
    ret = gpio_set_pull_mode(scl_io_, GPIO_PULLUP_ONLY);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "设置SCL上拉失败: %s", errorToString(ret));
        return ret;
    }
    
    ret = gpio_set_pull_mode(sda_io_, GPIO_PULLUP_ONLY);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "设置SDA上拉失败: %s", errorToString(ret));
        return ret;
    }
    
    // 生成停止条件
    gpio_set_level(scl_io_, 0);
    gpio_set_level(sda_io_, 0);
    vTaskDelay(pdMS_TO_TICKS(1));
    gpio_set_level(scl_io_, 1);
    vTaskDelay(pdMS_TO_TICKS(1));
    gpio_set_level(sda_io_, 1);
    vTaskDelay(pdMS_TO_TICKS(1));
    
    ESP_LOGI(TAG, "I2C总线重置完成");
    return ESP_OK;
}

bool I2cMaster::isBusIdle() const {
    if (!initialized_) {
        return false;
    }
    
    // GPIO读取是原子操作，且如果总线忙碌说明其他地方已有锁保护
    // 硬件I2C：ESP32驱动管理GPIO状态，线程安全
    // 软件I2C：如果正在传输，调用方已持有锁，不会并发调用此函数
    int scl_level = gpio_get_level(scl_io_);
    int sda_level = gpio_get_level(sda_io_);
    
    return (scl_level == 1 && sda_level == 1);
}

// 基础传输功能实现
esp_err_t I2cMaster::transmit(uint16_t device_address, const uint8_t* data, size_t size, int timeout_ms) {
    return internalTransmit(device_address, data, size, timeout_ms);
}

esp_err_t I2cMaster::transmit(uint16_t device_address, const std::vector<uint8_t>& data, int timeout_ms) {
    return transmit(device_address, data.data(), data.size(), timeout_ms);
}

esp_err_t I2cMaster::transmit(i2c_master_dev_handle_t dev_handle, const uint8_t* data, size_t size, int timeout_ms) {
    if (!initialized_ || type_ != I2CType::HARDWARE || dev_handle == nullptr) {
        ESP_LOGE(TAG, "设备句柄发送失败: 参数无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    if (data == nullptr || size == 0) {
        ESP_LOGE(TAG, "设备句柄发送失败: 数据无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    // i2c_master_transmit 已经是线程安全的，不需要额外锁
    esp_err_t ret = i2c_master_transmit(dev_handle, data, size, timeout_ms);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "设备句柄发送失败: %s", errorToString(ret));
    }
    
    return ret;
}

esp_err_t I2cMaster::receive(uint16_t device_address, uint8_t* buffer, size_t size, int timeout_ms) {
    return internalReceive(device_address, buffer, size, timeout_ms);
}

esp_err_t I2cMaster::receive(uint16_t device_address, std::vector<uint8_t>& data, size_t size, int timeout_ms) {
    data.resize(size);
    esp_err_t ret = receive(device_address, data.data(), size, timeout_ms);
    if (ret != ESP_OK) {
        data.clear();
    }
    return ret;
}

esp_err_t I2cMaster::receive(i2c_master_dev_handle_t dev_handle, uint8_t* buffer, size_t size, int timeout_ms) {
    if (!initialized_ || type_ != I2CType::HARDWARE || dev_handle == nullptr) {
        ESP_LOGE(TAG, "设备句柄接收失败: 参数无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    if (buffer == nullptr || size == 0) {
        ESP_LOGE(TAG, "设备句柄接收失败: 缓冲区无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    // i2c_master_receive 已经是线程安全的，不需要额外锁
    esp_err_t ret = i2c_master_receive(dev_handle, buffer, size, timeout_ms);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "设备句柄接收失败: %s", errorToString(ret));
    }
    
    return ret;
}

esp_err_t I2cMaster::transmitReceive(uint16_t device_address, const uint8_t* write_data, size_t write_size,
                              uint8_t* read_buffer, size_t read_size, int timeout_ms) {
    return internalTransmitReceive(device_address, write_data, write_size, read_buffer, read_size, timeout_ms);
}

esp_err_t I2cMaster::transmitReceive(uint16_t device_address, const std::vector<uint8_t>& write_data,
                              std::vector<uint8_t>& read_data, size_t read_size, int timeout_ms) {
    read_data.resize(read_size);
    esp_err_t ret = transmitReceive(device_address, write_data.data(), write_data.size(), 
                                   read_data.data(), read_size, timeout_ms);
    if (ret != ESP_OK) {
        read_data.clear();
    }
    return ret;
}

esp_err_t I2cMaster::transmitReceive(i2c_master_dev_handle_t dev_handle, const uint8_t* write_data, size_t write_size,
                              uint8_t* read_buffer, size_t read_size, int timeout_ms) {
    if (!initialized_ || type_ != I2CType::HARDWARE || dev_handle == nullptr) {
        ESP_LOGE(TAG, "设备句柄发送接收失败: 参数无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    if (write_data == nullptr || read_buffer == nullptr || 
        write_size == 0 || read_size == 0) {
        ESP_LOGE(TAG, "设备句柄发送接收失败: 数据无效");
        return ESP_ERR_INVALID_ARG;
    }
    
    // i2c_master_transmit_receive 已经是线程安全的，不需要额外锁
    esp_err_t ret = i2c_master_transmit_receive(dev_handle, write_data, write_size, 
                                      read_buffer, read_size, timeout_ms);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "设备句柄发送接收失败: %s", errorToString(ret));
    }
    
    return ret;
}

// 寄存器操作功能实现
esp_err_t I2cMaster::writeRegister(uint16_t device_address, uint8_t reg_addr, uint8_t data, int timeout_ms) {
    uint8_t write_data[] = {reg_addr, data};
    return transmit(device_address, write_data, sizeof(write_data), timeout_ms);
}

esp_err_t I2cMaster::writeRegister(uint16_t device_address, uint8_t reg_addr, const std::vector<uint8_t>& data, int timeout_ms) {
    std::vector<uint8_t> write_data;
    write_data.reserve(1 + data.size());
    write_data.push_back(reg_addr);
    write_data.insert(write_data.end(), data.begin(), data.end());
    return transmit(device_address, write_data, timeout_ms);
}

esp_err_t I2cMaster::writeRegister(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t data, int timeout_ms) {
    uint8_t write_data[] = {reg_addr, data};
    return transmit(dev_handle, write_data, sizeof(write_data), timeout_ms);
}

esp_err_t I2cMaster::readRegister(uint16_t device_address, uint8_t reg_addr, uint8_t& data, int timeout_ms) {
    esp_err_t ret = transmitReceive(device_address, &reg_addr, 1, &data, 1, timeout_ms);
    return ret;
}

esp_err_t I2cMaster::readRegister(uint16_t device_address, uint8_t reg_addr, std::vector<uint8_t>& data, size_t size, int timeout_ms) {
    return transmitReceive(device_address, std::vector<uint8_t>{reg_addr}, data, size, timeout_ms);
}

esp_err_t I2cMaster::readRegister(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t& data, int timeout_ms) {
    esp_err_t ret = transmitReceive(dev_handle, &reg_addr, 1, &data, 1, timeout_ms);
    return ret;
}

esp_err_t I2cMaster::readRegister(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t* buffer, size_t size, int timeout_ms) {
    return transmitReceive(dev_handle, &reg_addr, 1, buffer, size, timeout_ms);
}

// 内部传输方法实现（优化版本）
esp_err_t I2cMaster::internalTransmit(uint16_t device_address, const uint8_t* data, size_t size, int timeout_ms) {
    if (!initialized_) {
        ESP_LOGE(TAG, "I2C未初始化");
        return ESP_ERR_INVALID_STATE;
    }
    
    if (data == nullptr || size == 0) {
        ESP_LOGE(TAG, "数据指针不能为空且大小不能为0");
        return ESP_ERR_INVALID_ARG;
    }
    
    esp_err_t ret;
    if (type_ == I2CType::HARDWARE) {
        // 只在获取/创建设备句柄时加锁
        i2c_master_dev_handle_t dev_handle;
        {
            std::lock_guard<std::mutex> lock(mutex_);
            dev_handle = getOrCreateDeviceHandle(device_address);
        }
        
        if (dev_handle == nullptr) {
            ESP_LOGE(TAG, "无法获取设备句柄");
            return ESP_ERR_INVALID_STATE;
        }
        
        // i2c_master_transmit 已经是线程安全的，不需要锁
        ret = i2c_master_transmit(dev_handle, data, size, timeout_ms);
    } else {
        // 软件I2C需要完整的锁保护
        std::lock_guard<std::mutex> lock(mutex_);
        ret = softI2CStart();
        if (ret == ESP_OK) {
            ret = softI2CWriteByte((device_address << 1) | 0);  // 写操作
            if (ret == ESP_OK) {
                for (size_t i = 0; i < size && ret == ESP_OK; i++) {
                    ret = softI2CWriteByte(data[i]);
                }
            }
            softI2CStop();
        }
    }
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C发送失败: %s", errorToString(ret));
    }
    
    return ret;
}

esp_err_t I2cMaster::internalReceive(uint16_t device_address, uint8_t* buffer, size_t size, int timeout_ms) {
    if (!initialized_) {
        ESP_LOGE(TAG, "I2C未初始化");
        return ESP_ERR_INVALID_STATE;
    }
    
    if (buffer == nullptr || size == 0) {
        ESP_LOGE(TAG, "缓冲区指针不能为空且大小不能为0");
        return ESP_ERR_INVALID_ARG;
    }
    
    esp_err_t ret;
    if (type_ == I2CType::HARDWARE) {
        // 只在获取/创建设备句柄时加锁
        i2c_master_dev_handle_t dev_handle;
        {
            std::lock_guard<std::mutex> lock(mutex_);
            dev_handle = getOrCreateDeviceHandle(device_address);
        }
        
        if (dev_handle == nullptr) {
            ESP_LOGE(TAG, "无法获取设备句柄");
            return ESP_ERR_INVALID_STATE;
        }
        
        // i2c_master_receive 已经是线程安全的，不需要锁
        ret = i2c_master_receive(dev_handle, buffer, size, timeout_ms);
    } else {
        // 软件I2C需要完整的锁保护
        std::lock_guard<std::mutex> lock(mutex_);
        ret = softI2CStart();
        if (ret == ESP_OK) {
            ret = softI2CWriteByte((device_address << 1) | 1);  // 读操作
            if (ret == ESP_OK) {
                for (size_t i = 0; i < size; i++) {
                    buffer[i] = softI2CReadByte(i < (size - 1));  // 最后一个字节发送NACK
                }
            }
            softI2CStop();
        }
    }
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C接收失败: %s", errorToString(ret));
    }
    
    return ret;
}

esp_err_t I2cMaster::internalTransmitReceive(uint16_t device_address, const uint8_t* write_data, size_t write_size,
                                      uint8_t* read_buffer, size_t read_size, int timeout_ms) {
    if (!initialized_) {
        ESP_LOGE(TAG, "I2C未初始化");
        return ESP_ERR_INVALID_STATE;
    }
    
    if (write_data == nullptr || read_buffer == nullptr || 
        write_size == 0 || read_size == 0) {
        ESP_LOGE(TAG, "数据指针不能为空且大小不能为0");
        return ESP_ERR_INVALID_ARG;
    }
    
    esp_err_t ret;
    if (type_ == I2CType::HARDWARE) {
        // 只在获取/创建设备句柄时加锁
        i2c_master_dev_handle_t dev_handle;
        {
            std::lock_guard<std::mutex> lock(mutex_);
            dev_handle = getOrCreateDeviceHandle(device_address);
        }
        
        if (dev_handle == nullptr) {
            ESP_LOGE(TAG, "无法获取设备句柄");
            return ESP_ERR_INVALID_STATE;
        }
        
        // i2c_master_transmit_receive 已经是线程安全的，不需要锁
        ret = i2c_master_transmit_receive(dev_handle, write_data, write_size, 
                                         read_buffer, read_size, timeout_ms);
    } else {
        // 软件I2C需要完整的锁保护
        std::lock_guard<std::mutex> lock(mutex_);
        ret = softI2CStart();
        if (ret == ESP_OK) {
            // 写阶段
            ret = softI2CWriteByte((device_address << 1) | 0);  // 写操作
            if (ret == ESP_OK) {
                for (size_t i = 0; i < write_size && ret == ESP_OK; i++) {
                    ret = softI2CWriteByte(write_data[i]);
                }
            }
            
            if (ret == ESP_OK) {
                // 重新开始读操作
                ret = softI2CStart();
                if (ret == ESP_OK) {
                    ret = softI2CWriteByte((device_address << 1) | 1);  // 读操作
                    if (ret == ESP_OK) {
                        for (size_t i = 0; i < read_size; i++) {
                            read_buffer[i] = softI2CReadByte(i < (read_size - 1));
                        }
                    }
                }
            }
            softI2CStop();
        }
    }
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C发送接收失败: %s", errorToString(ret));
    }
    
    return ret;
}

// 工具函数实现
const char* I2cMaster::errorToString(esp_err_t err) {
    switch (err) {
        case ESP_OK: return "成功";
        case ESP_ERR_INVALID_ARG: return "无效参数";
        case ESP_ERR_NO_MEM: return "内存不足";
        case ESP_ERR_NOT_FOUND: return "未找到";
        case ESP_ERR_TIMEOUT: return "超时";
        case ESP_FAIL: return "一般错误";
        case ESP_ERR_INVALID_STATE: return "无效状态";
        case ESP_ERR_NOT_SUPPORTED: return "不支持";
        default: return "未知错误";
    }
}

bool I2cMaster::isValidDeviceAddress(uint16_t address, I2CAddrLength addr_len) {
    if (addr_len == I2CAddrLength::ADDR_7BIT) {
        return address <= 0x7F;
    } else {
        return address <= 0x3FF;
    }
}

// 软件I2C内部实现
void I2cMaster::softI2CDelay() {
    if (sw_bus_handle_) {
        esp_rom_delay_us(sw_bus_handle_->freq_delay_us);
    }
}

void I2cMaster::softI2CSetSCL(bool level) {
    gpio_set_level(sw_bus_handle_->scl_pin, level ? 1 : 0);
    softI2CDelay();
}

void I2cMaster::softI2CSetSDA(bool level) {
    gpio_set_level(sw_bus_handle_->sda_pin, level ? 1 : 0);
    softI2CDelay();
}

bool I2cMaster::softI2CGetSDA() {
    return gpio_get_level(sw_bus_handle_->sda_pin) != 0;
}

esp_err_t I2cMaster::softI2CStart() {
    if (!sw_bus_handle_ || !sw_bus_handle_->initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    // START条件：SCL为高时，SDA从高变低
    softI2CSetSCL(true);
    softI2CSetSDA(true);
    softI2CSetSDA(false);
    softI2CSetSCL(false);
    
    return ESP_OK;
}

esp_err_t I2cMaster::softI2CStop() {
    if (!sw_bus_handle_ || !sw_bus_handle_->initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    // STOP条件：SCL为高时，SDA从低变高
    softI2CSetSCL(false);
    softI2CSetSDA(false);
    softI2CSetSCL(true);
    softI2CSetSDA(true);
    
    return ESP_OK;
}

esp_err_t I2cMaster::softI2CWriteByte(uint8_t data) {
    if (!sw_bus_handle_ || !sw_bus_handle_->initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    // 发送8位数据
    for (int i = 7; i >= 0; i--) {
        softI2CSetSCL(false);
        softI2CSetSDA((data >> i) & 1);
        softI2CSetSCL(true);
    }
    
    // 读取ACK
    softI2CSetSCL(false);
    softI2CSetSDA(true);  // 释放SDA线
    softI2CSetSCL(true);
    
    bool ack = !softI2CGetSDA();  // ACK为低电平
    softI2CSetSCL(false);
    
    return ack ? ESP_OK : ESP_FAIL;
}

uint8_t I2cMaster::softI2CReadByte(bool ack) {
    if (!sw_bus_handle_ || !sw_bus_handle_->initialized) {
        return 0;
    }
    
    uint8_t data = 0;
    
    // 读取8位数据
    softI2CSetSDA(true);  // 释放SDA线
    for (int i = 7; i >= 0; i--) {
        softI2CSetSCL(false);
        softI2CSetSCL(true);
        if (softI2CGetSDA()) {
            data |= (1 << i);
        }
    }
    
    // 发送ACK/NACK
    softI2CSetSCL(false);
    softI2CSetSDA(!ack);  // ACK为低电平，NACK为高电平
    softI2CSetSCL(true);
    softI2CSetSCL(false);
    
    return data;
} 